#include<stdio.h>
//#include<Mesh/Mesh_Frame.h>
//#include<Mesh/lib_cell_Iterator.h>
#include<Mesh_IO/Mesh_IO.h>
//#include<Algorithm/Algorithm1.h>
//#include<Algorithm/Algorithm2.h>
#include<tool/libcell_tools_formats.h>
#include<tool/libcell_tools_view.h>
//#include<Algorithm/Harmonic.h>
//#include <Algorithm/Conformal.h>
#include<tool/libcell_tools_octree.h>
#include<tool/libcell_tools_algorithm.h>
#include<Mesh/libcell_iterator.h>
#include <Algorithm/qem.h>
#include<libcell_macros.h>
#define _ReadOff_   LibCell_ReadOff_
#define _ReadCell_  LibCell_ReadCell_
#define _ReadPly_ LibCell_ReadPly_

#define _WriteCell_ LibCell_WriteCell_

void test_tools_algorithm()
{
    Mesh m,m1;
    Mesh_init(&m);Mesh_init(&m1);

    _ReadOff_(&m1,"face.off",3);
    _ReadCell_(&m,"cube.cell");
    printf("num f :%d\n",m.num_v(&m));
    template_v* v=m.get_vertexp(&m,0);
    Node* node=node_copy(v->faces);
    //printf("node size:%d\n",node_size(node));
    Int_RB_Tree*tree=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree);
    Int_RB_Tree* tree1=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree1);
    Mesh_cut_along_the_curvef(&m,node,tree,tree1);
    printf("num f :%d\n",m.num_v(&m));
    for(auto vit=m.v_begin(&m);vit!=m.v_end(&m);vit++)
    {
        printf("%lf %lf %lf\n",quote(vit)->point[0],quote(vit)->point[1],quote(vit)->point[2]);
    }

    int_rb_tree_free_value(tree);
    int_rb_tree_free(tree);
    int_rb_tree_free(tree1);
    Mesh_free(&m); Mesh_free(&m1);
}
void test_read_ply()
{
    Mesh m;
    Mesh_init(&m);
    _ReadPly_(&m,"bunny.ply" );
    Mesh_free(&m);
}
void my_sscanf(char * str,int* i1,int* i2,int *i3)
{
    char str1[100]={NULL};
    int temp_i=0;
    sscanf(&str[temp_i],"%[^/]",str1);
    //printf("%s ",str1);
    sscanf(str1,"%d",i1);
    temp_i=temp_i+strlen(str1)+1;
    int i=sscanf(&str[temp_i],"%[^/]",str1);
    if(i==1)
    {
        temp_i=temp_i+strlen(str1)+1;
        //printf("%s %d ",str1,i);
        sscanf(str1,"%d",i2); 
    }
    else
    {
        *i2=-1;
        temp_i=temp_i+1;
       // printf(" ");
    }
    i=sscanf(&str[temp_i],"%[^/]",str1);
    if(i==1)
    {
        sscanf(str1,"%d",i3);
        // printf("%s \n",str1);
    }
    else
    {
        *i3=-1;
       // printf("\n");
    } 
}
double piecewise_map(double x);


double compute_distance(void*,double* );

void* fitting_plane(double**,int len);

void* compute_plane(double**,int len);




int * get_random_sequence(int len,int n)
{
    int* re=(int*)malloc(sizeof(int)*n);
    double initx=(rand()%900000)/900000.0;

    for(int i=0;i<n;i++)
    {
        //re[i]=(rand()%900000)/900000.0;
        initx= piecewise_map(initx);
        re[i]=floor( initx*(len-i));

    }

    for(int i=1;i<n;i++)
    {
        for(int j=0;j<i;j++)
        {
            if(re[i]>=re[j])
            {
                re[i]++;
            }
        } 
    } 


    return re;
}


double piecewise_map(double x)
{
    if(x<=0.00000001)
    {
        return 0.00001;
    }
    else if(x<0.30)
    {
        return x/0.30;
    }
    else if(x<0.5)
    {
        return (x-0.30)/(0.5-0.30);
    }
    else if(x<0.70)
    {
        return (1-0.30-x)/(0.5-0.30);
    }
    else{
        return (1-x)/0.30;
    }
    return 0;

}

void compute_ransac(double (*c)(void*,double*) ,void* (* f)(double**,int),
    void* (*cp)(double**,int),double**S,int len,int len1,double threshold)
{

    double** s= (double**)malloc(sizeof(double*)*len1);
    int*seq=get_random_sequence(len,len1);
    for(int i=0;i<len1;i++)
    {
        s[i]=S[seq[i]];           
    }
    free(seq);

    
    void * p=f(s,len1);
    int sum=0;

    for(int i=0;i<len;i++)
    {
        if(c(p,S[i])<threshold)
        {
            sum++;
        }
    } 


    free(p);  
    
    free(s);
    // for()
    // {

    // } 
}
#include<Math/LB_Math.h>
#include <Matrix/LB_Matrix.h>

static void gauss_bonnet(Mesh* m)
{

    double re=0,re1=0;
    LB_Matrix* mat=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(mat,3,3);
    double* data=(double*)(mat->data);

    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        double sum1=0;
        for(auto vfit=m->vf_begin(m,*quote(vit));vfit!=m->vf_end(m,*quote(vit));vfit++)
        {
            double data1[4][3]={0};
            template_c* c1=NULL,*c2=NULL;
            
            if(quote(vfit)->halffaces[0]->vertices[0]==quote(vit))
            {
                c1=quote(vfit)->halffaces[0]->cell;
                c2=quote(vfit)->halffaces[1]->cell;
            }
            else
            {
                c2=quote(vfit)->halffaces[0]->cell;
                c1=quote(vfit)->halffaces[1]->cell;
            }
            for(int i=0;i<c1->vertices_size;i++)
            {
                data1[i][0]=c1->vertices[i]->point[0];
                data1[i][1]=c1->vertices[i]->point[1];
                data1[i][2]=c1->vertices[i]->point[2];
                if(c1->vertices[i]!=quote(vit) )
                {
                    double p[3]={data1[i][0]-quote(vit)->point[0],
                        data1[i][1]-quote(vit)->point[1],data1[i][2]-quote(vit)->point[2] };
                    normalize(p,3);
                    data1[i][0]=quote(vit)->point[0]+p[0];
                    data1[i][1]=quote(vit)->point[1]+p[1];
                    data1[i][2]=quote(vit)->point[2]+p[2];
                }
            }
            for(int i=0;i<c2->vertices_size;i++)
            {
                if(m->vertex_is_in_cell(m,c2->vertices[i],c1))
                {
                    continue;
                }
                data1[3][0]=c2->vertices[i]->point[0];
                data1[3][1]=c2->vertices[i]->point[1];
                data1[3][2]=c2->vertices[i]->point[2];
                double p[3]={data1[3][0]-quote(vit)->point[0],
                        data1[3][1]-quote(vit)->point[1],data1[3][2]-quote(vit)->point[2] };
                normalize(p,3);
                data1[3][0]=quote(vit)->point[0]+p[0];
                data1[3][1]=quote(vit)->point[1]+p[1];
                data1[3][2]=quote(vit)->point[2]+p[2];
            }
            for(int i=0;i<3;i++)
            {
                for(int j=0;j<3;j++)
                {
                    data[i*3+j]=data1[i+1][j]-data1[0][j];
                }
            }

            double* det=(double*)(mat->det(mat));

            sum1+=(*det);

            SAFE_FREE(det);
        }
        printf("sum1 :%lf \n",sum1);

        re1+=sum1;

        double sum=0;
        for(auto vcit=m->vc_begin(m,*quote(vit));vcit!=m->vc_end(m,*quote(vit));vcit++)
        {
            template_v* vs[2]={NULL};
            int k=0;
            for(int i=0;i<quote(vcit)->vertices_size;i++)
            {
                if(quote(vcit)->vertices[i]!=quote(vit) )
                {
                    vs[k]=quote(vcit)->vertices[i];
                    k++;
                }
            }

            double dir1[3]={vs[0]->point[0]-quote(vit)->point[0],
                vs[0]->point[1]-quote(vit)->point[1], vs[0]->point[2]-quote(vit)->point[2] };
            double dir2[3]={vs[1]->point[0]-quote(vit)->point[0],
                vs[1]->point[1]-quote(vit)->point[1], vs[1]->point[2]-quote(vit)->point[2] };

            normalize(dir1,3);normalize(dir2,3);
            sum+=acos(inner_product(dir1,dir2,3));
        }
        re+=(2*M_PI-sum);

    }
    printf("gauss bonnet gongshi:%lf %lf\n",re,re1 ) ;

}

void test_read_obj()
{
    Mesh mesh;
    Mesh_init(&mesh);
    printf("begin read obj\n");
    
    _ReadObj_ (&mesh,"cat.obj");



    Mesh_free(&mesh); 
}

void test_qem()
{
    printf("begin qem\n");
    Mesh mesh;
    Mesh_init(&mesh);
    _ReadOff_(&mesh,"face.off",3);
    printf("num v :%d num f:%d num c:%d\n",mesh.num_v(&mesh),mesh.num_f(&mesh),mesh.num_c(&mesh));


    Int_RB_Tree* tree=qem_init_qi(&mesh);

    CHeap*ch =compute_qem_all_edge_position_and_energy(&mesh,tree);
    for(int i=0;i<1000&&ch->size>0;i++)
    {
        void* value=pop_cheap(ch);
        Edge_Energy*ee=*((Edge_Energy**)value);
        if(qem_collapse_edge(&mesh,ee,tree,ch)==NULL)
        {
            i--;
        }
        edge_energy_free(ee);
        free(value);
    } 
    
    printf("num v :%d num f:%d num c:%d\n",mesh.num_v(&mesh),mesh.num_f(&mesh),mesh.num_c(&mesh));
    _WriteCell_(&mesh,"qemmesh.cell");

    int_rb_tree_free(tree);
    Mesh_free(&mesh);
    printf("end qem\n");

}


int main(int argc,char**argv)
{

    test_qem();

    double initx=0.121;

    for(int i=0;i<600;i++)
    {
        initx= piecewise_map(initx);
        printf("%lf ",initx);
    }
    test_read_obj();

    test_read_ply();
    test_tools_algorithm();
   //  FILE * infile=fopen("test.txt","r");
   //  int t=0;
   //  fscanf(infile,"%d",&t);
   //  printf("%d\n",t ); 
   //  char c;
   //  fscanf(infile,"%c",&c);
   //  printf("%c\n",c);  
   //  fscanf(infile,"%c",&c);
   //  printf("%c\n",c);  
   //  fscanf(infile,"%d",&t);
   //  printf("%d\n",t);
    printf("begin read cell\n");
    for(int i=0;i<1;i++)
    {
        Mesh m;
        Mesh_init(&m);
        _ReadCell_ (&m,"bone.cell");

        // _ReadOff_(&m,"bunny.off",3);
        // _WriteCell_(&m,"bunny.cell");   
       Mesh_free(&m); 
    }
    printf("end read cell\n");
    

    
    char s[200]="f 135//135 134/228 222/229/222 223/230/223";
    
    char str[100]={NULL};
    
    int temp_i=2;
    while(sscanf(&s[temp_i],"%s",str)!=EOF)
    { 
        printf("%s %d\n",str,strlen(str)); 
        int ii1=0,ii2=0,ii3=0;
        char ss1[100]={NULL},ss2[100]={NULL};
        //sscanf(str,"%d[^/]%d[^/]",&ii1,&ii2); 
        my_sscanf(str,&ii1,&ii2,&ii3);
        printf("%d %d %d\n",ii1,ii2,ii3);    
        temp_i=temp_i+strlen(str)+1; 
        memset(str,0,sizeof(char)*100);
    }

    // Mesh m;
    // Mesh_init(&m);
    // printf("%d\n",m.num_c(&m) );

    // objtooff("bone_scan1.obj");

 /*   mpf_set_default_prec(200);
    Mesh mesh,mesh1;
    Mesh_init(&mesh);Mesh_init(&mesh1);
  
    _ReadCell_(&mesh,"cube.cell");
    template_v v0=mesh.get_vertex(&mesh,0);
    template_f f0=mesh.get_face(&mesh,0);
    // mesh.delete_vertex(&mesh,v0,true);
    mesh.delete_face(&mesh,f0,true);
    double M[3][3],M1[3][2];
    M[0][0]=21.0;M[0][1]=-2.41;M[0][2]=-212.97;
    M[1][0]=1.001;M[1][1]=-0.51;M[1][2]=-2.4;
    M[2][0]=2.9;M[2][1]=92.25;M[2][2]=12.23;
    M1[0][0]=1;M1[0][1]=0;
    M1[1][0]=1;M1[1][1]=0;
    M1[2][0]=0;M1[2][1]=1;
    Tensors_Algebra_System*tas=(Tensors_Algebra_System*)malloc(sizeof(Tensors_Algebra_System));
    Tensors_Algebra_System_mpf_init(tas,3);
    Tensors_Algebra_System* tas1=(Tensors_Algebra_System*)malloc(sizeof(Tensors_Algebra_System));
    Tensors_Algebra_System_double_init(tas1,3);
    Tensor* t=tas1->T_create_matrix(tas1,(double*)M,3,3);
    //tas1->print_T(t);
    Tensor* t1=tas1->T_create_matrix(tas1,(double*)M1,3,2);
    tas1->print_T(t1);
    Tensor* t2=W_Tensor_Contraction(tas1,t,t1,1,0);
    tas1->print_T(t2);

    //tensor_double_print_self(t); 
    
    //test_area();
    //test_convex();
    test_delauny();
    */
    //test_cell();
    //printf("%f\n",area_simplex_double(M,3,3));
    //Tensor*t=Anti_tensor_mpf_from_v(tas,M,3,3);
    //tensor_mpf_print_self(t);
    //gmp_printf("%.Ff\n",(__mpf_struct*)tas->T_norm(tas,t));
     
   // mesh.printself(&mesh);
 //   printf("v:%d,f:%d ,c:%d\n",mesh1.num_v(&mesh1),mesh1.num_f(&mesh1),mesh1.num_c(&mesh1));

   /* for(auto it=mesh.cells.begin();it!=mesh.cells.end();it++)
    {
        //printf("d");
    
    }*/
   // _ReadCell_(&mesh,"hand.cell");
 
    //objtooff("cat.obj"); 
    //pcdtooff("mypcd.pcd");
    //pcdtooff("tree1.pcd");
    // Mesh pm;
    // Mesh_init(&pm);
    // _ReadPly_(&pm, "luozhou_cloud.ply");
    //  Mesh pm1;
    // Mesh_init(&pm1);
    // _ReadObj_(&pm1, "checkerboard.obj");

    Mesh pm2;
    Mesh_init(&pm2);
    printf("begin read off\n");
    _ReadOff_(&pm2,"lawen.off",3); 

    // gauss_bonnet(&pm2);
    
    //_ReadPly_
    //_ReadPly_(&pm, "bunny.ply");


    printf("end\n");
    return 0;
}
